
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       CompassCalibrator.c
  * @author     baiyang
  * @date       2021-12-11
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "CompassCalibrator.h"
#include "sensor_compass.h"


#include <stdio.h>
#include <stdint.h>
#include <rtthread.h>

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>
#include <common/time/gp_time.h>
#include <common/gp_math/gp_geodesic_grid.h>
#include <common/gp_math/gp_matrix_alg.h>
#include <common/geo/declination.h>
#include <gcs_mavlink/gcs.h>
/*-----------------------------------macro------------------------------------*/
#define COMPASS_CAL_GCS_SEND_TEXT(severity, format, ...) gcs_send_text(severity, format, ##__VA_ARGS__)

#define FIELD_RADIUS_MIN 150
#define FIELD_RADIUS_MAX 950

#define COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(__X) ((int16_t)math_constrain_float(roundf(__X*8.0f), INT16_MIN, INT16_MAX))
#define COMPASS_CAL_SAMPLE_SCALE_TO_FLOAT(__X) (__X/8.0f)
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void pull_sample(CompassCalibrator *cal);

static bool set_status(CompassCalibrator *cal, enum CompassCalStatus status);
static void reset_state(CompassCalibrator *cal);
static void initialize_fit(CompassCalibrator *cal);
static float calc_mean_squared_residuals(CompassCalibrator *cal, const CompassCalParam* params);
static float calc_residual(const Vector3f_t* sample, const CompassCalParam* params);
static void thin_samples(CompassCalibrator *cal);
static bool accept_sample(CompassCalibrator *cal, const CompassSample* sample, uint16_t skip_index);
static bool accept_sample2(CompassCalibrator *cal, const Vector3f_t* sample, uint16_t skip_index);
static void update_completion_mask(CompassCalibrator *cal);
static void update_completion_mask2(CompassCalibrator *cal, const Vector3f_t* v);
static bool _running(CompassCalibrator *cal);
static void update_cal_settings(CompassCalibrator *cal);
static void update_cal_status(CompassCalibrator *cal);
static void update_cal_report(CompassCalibrator *cal);
static bool _fitting(CompassCalibrator *cal);
static void calc_initial_offset(CompassCalibrator *cal);
static void run_sphere_fit(CompassCalibrator *cal);
static void calc_sphere_jacob(CompassCalibrator *cal, const Vector3f_t* sample, const CompassCalParam* params, float* ret);
static bool fit_acceptable(CompassCalibrator *cal);
static bool fix_radius(CompassCalibrator *cal);
static bool calculate_orientation(CompassCalibrator *cal);
static Vector3f_t calculate_earth_field(CompassCalibrator *cal, CompassSample *sample, enum RotationEnum r);
static bool right_angle_rotation(enum RotationEnum r);
static void run_ellipsoid_fit(CompassCalibrator *cal);
static void calc_ellipsoid_jacob(CompassCalibrator *cal, const Vector3f_t* sample, const CompassCalParam* params, float* ret);


static Vector3f_t CompassSample_get(const CompassSample *sample_buffer);
static void CompassSample_set(CompassSample *sample_buffer, const Vector3f_t *in);
static void AttitudeSample_set_from_ahrs(CompassCalAttSample *att_sam);
static matrix3f_t AttitudeSample_get_rotmat(CompassCalAttSample *att_sam);

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void compasscal_ctor(CompassCalibrator *cal, int8_t instance)
{
    if (cal == NULL) {
        return;
    }

    rt_memset(cal, 0, sizeof(CompassCalibrator));

    cal->_tolerance = 5.0f;

    // setup a name
    char name[9] = {0};
    snprintf(name, sizeof(name), "calst%d", instance);
    rt_mutex_init(&cal->state_sem, name, RT_IPC_FLAG_FIFO);

    snprintf(name, sizeof(name), "calsa%d", instance);
    rt_mutex_init(&cal->sample_sem, name, RT_IPC_FLAG_FIFO);

    set_status(cal, COMPASS_CAL_NOT_STARTED);
}

// Request to cancel calibration 
void compasscal_stop(CompassCalibrator *cal)
{
    rt_mutex_take(&cal->state_sem, RT_WAITING_FOREVER);

    cal->_requested_status = COMPASS_CAL_NOT_STARTED;
    cal->_status_set_requested = true;

    rt_mutex_release(&cal->state_sem);
}

void compasscal_set_orientation(CompassCalibrator *cal, enum RotationEnum orientation, bool is_external, bool fix_orientation, bool always_45_deg)
{
    rt_mutex_take(&cal->state_sem, RT_WAITING_FOREVER);

    cal->cal_settings.check_orientation = true;
    cal->cal_settings.orientation = orientation;
    cal->cal_settings.orig_orientation = orientation;
    cal->cal_settings.is_external = is_external;
    cal->cal_settings.fix_orientation = fix_orientation;
    cal->cal_settings.always_45_deg = always_45_deg;

    rt_mutex_release(&cal->state_sem);
}

void compasscal_start(CompassCalibrator *cal, bool retry, float delay, uint16_t offset_max, uint8_t compass_idx, float tolerance)
{
    if (compass_idx > COMPASS_MAX_INSTANCES) {
        return;
    }

    rt_mutex_take(&cal->state_sem, RT_WAITING_FOREVER);

    // Don't do this while we are already started
    if (_running(cal)) {
        rt_mutex_release(&cal->state_sem);
        return;
    }
    cal->cal_settings.offset_max = offset_max;
    cal->cal_settings.attempt = 1;
    cal->cal_settings.retry = retry;
    cal->cal_settings.delay_start_sec = delay;
    cal->cal_settings.start_time_ms = time_millis();
    cal->cal_settings.compass_idx = compass_idx;
    cal->cal_settings.tolerance = tolerance;

    // Request status change to Waiting to start
    cal->_requested_status = COMPASS_CAL_WAITING_TO_START;
    cal->_status_set_requested = true;

    rt_mutex_release(&cal->state_sem);
}

// Record point mag sample and associated attitude sample to intermediate struct
void compasscal_new_sample(CompassCalibrator *cal, const Vector3f_t* sample)
{
    rt_mutex_take(&cal->sample_sem, RT_WAITING_FOREVER);

    CompassSample_set(&cal->_last_sample, sample);
    AttitudeSample_set_from_ahrs(&cal->_last_sample.att);
    cal->_new_sample = true;

    rt_mutex_release(&cal->sample_sem);
}

bool compasscal_failed(CompassCalibrator *cal)
{
    rt_mutex_take(&cal->state_sem, RT_WAITING_FOREVER);

    bool result = (cal->cal_state.status == COMPASS_CAL_FAILED ||
            cal->cal_state.status == COMPASS_CAL_BAD_ORIENTATION || 
            cal->cal_state.status == COMPASS_CAL_BAD_RADIUS);

    rt_mutex_release(&cal->state_sem);

    return result;
}

bool compasscal_running(CompassCalibrator *cal)
{
    rt_mutex_take(&cal->state_sem, RT_WAITING_FOREVER);
    bool result = (cal->cal_state.status == COMPASS_CAL_RUNNING_STEP_ONE || cal->cal_state.status == COMPASS_CAL_RUNNING_STEP_TWO);
    rt_mutex_release(&cal->state_sem);

    return result;
}

const struct CompassCalReport compasscal_get_report(CompassCalibrator *cal)
{
    rt_mutex_take(&cal->state_sem, RT_WAITING_FOREVER);
    const struct CompassCalReport report = cal->cal_report;
    rt_mutex_release(&cal->state_sem);

    return report;
}

const struct CompassCalState compasscal_get_state(CompassCalibrator *cal)
{
    rt_mutex_take(&cal->state_sem, RT_WAITING_FOREVER);
    const struct CompassCalState state = cal->cal_state;
    rt_mutex_release(&cal->state_sem);

    return state;
}

// convert index to rotation, this allows to skip some rotations
enum RotationEnum compasscal_auto_rotation_index(uint8_t n)
{
    if (n < ROTATION_PITCH_180_YAW_90) {
        return (enum RotationEnum)n;
    }
    // ROTATION_PITCH_180_YAW_90 (26) is the same as ROTATION_ROLL_180_YAW_90 (10)
    // ROTATION_PITCH_180_YAW_270 (27) is the same as ROTATION_ROLL_180_YAW_270 (14)
    n += 2;
    if (n < ROTATION_ROLL_90_PITCH_68_YAW_293) {
        return (enum RotationEnum)n;
    }
    // ROTATION_ROLL_90_PITCH_68_YAW_293 is for solo leg compass, not expecting to see this in other vehicles
    n += 1;
    if (n < ROTATION_PITCH_7) {
        return (enum RotationEnum)n;
    }
    // ROTATION_PITCH_7 is too close to ROTATION_NONE to tell the difference in compass cal
    n += 1;
    if (n < ROTATION_MAX) {
        return (enum RotationEnum)n;
    }

    return ROTATION_NONE;
};

void compasscal_update(CompassCalibrator *cal)
{

    //pickup samples from intermediate struct
    pull_sample(cal);

    {
        rt_mutex_take(&cal->state_sem, RT_WAITING_FOREVER);
        //update_settings
        if (!compasscal_running(cal)) {
            update_cal_settings(cal);
        }

        //update requested state
        if (cal->_status_set_requested) {
            cal->_status_set_requested = false;
            set_status(cal, cal->_requested_status);
        }
        //update report and status
        update_cal_status(cal);
        update_cal_report(cal);

        rt_mutex_release(&cal->state_sem);
    }

    // collect the minimum number of samples
    if (!_fitting(cal)) {
        return;
    }

    if (cal->_status == COMPASS_CAL_RUNNING_STEP_ONE) {
        if (cal->_fit_step >= 10) {
            if (math_flt_equal(cal->_fitness, cal->_initial_fitness) || isnan(cal->_fitness)) {  // if true, means that fitness is diverging instead of converging
                set_status(cal, COMPASS_CAL_FAILED);
            } else {
                set_status(cal, COMPASS_CAL_RUNNING_STEP_TWO);
            }
        } else {
            if (cal->_fit_step == 0) {
                calc_initial_offset(cal);
            }
            run_sphere_fit(cal);
            cal->_fit_step++;
        }
    } else if (cal->_status == COMPASS_CAL_RUNNING_STEP_TWO) {
        if (cal->_fit_step >= 35) {
            if (fit_acceptable(cal) && fix_radius(cal) && calculate_orientation(cal)) {
                set_status(cal, COMPASS_CAL_SUCCESS);
            } else {
                set_status(cal, COMPASS_CAL_FAILED);
            }
        } else if (cal->_fit_step < 15) {
            run_sphere_fit(cal);
            cal->_fit_step++;
        } else {
            run_ellipsoid_fit(cal);
            cal->_fit_step++;
        }
    }
}

/////////////////////////////////////////////////////////////
////////////////////// PRIVATE METHODS //////////////////////
/////////////////////////////////////////////////////////////
static void pull_sample(CompassCalibrator *cal)
{
    CompassSample mag_sample;
    {
        rt_mutex_take(&cal->sample_sem, RT_WAITING_FOREVER);
        if (!cal->_new_sample) {
            rt_mutex_release(&cal->sample_sem);
            return;
        }
        if (cal->_status == COMPASS_CAL_WAITING_TO_START) {
            set_status(cal, COMPASS_CAL_RUNNING_STEP_ONE);
        }
        cal->_new_sample = false;
        mag_sample = cal->_last_sample;

        rt_mutex_release(&cal->sample_sem);
    }

    Vector3f_t sample = CompassSample_get(&mag_sample);

    if (_running(cal) && cal->_samples_collected < COMPASS_CAL_NUM_SAMPLES && accept_sample2(cal, &sample, UINT16_MAX)) {
        update_completion_mask2(cal, &sample);
        cal->_sample_buffer[cal->_samples_collected] = mag_sample;
        cal->_samples_collected++;
    }
}


static bool set_status(CompassCalibrator *cal, enum CompassCalStatus status)
{
    if (status != COMPASS_CAL_NOT_STARTED && cal->_status == status) {
        return true;
    }

    switch (status) {
        case COMPASS_CAL_NOT_STARTED:
            reset_state(cal);
            cal->_status = COMPASS_CAL_NOT_STARTED;
            if (cal->_sample_buffer != NULL) {
                rt_free(cal->_sample_buffer);
                cal->_sample_buffer = NULL;
            }
            return true;

        case COMPASS_CAL_WAITING_TO_START:
            reset_state(cal);
            cal->_status = COMPASS_CAL_WAITING_TO_START;
            set_status(cal, COMPASS_CAL_RUNNING_STEP_ONE);
            return true;

        case COMPASS_CAL_RUNNING_STEP_ONE:
            if (cal->_status != COMPASS_CAL_WAITING_TO_START) {
                return false;
            }

            // on first attempt delay start if requested by caller
            if (cal->_attempt == 1 && (time_millis()-cal->_start_time_ms)*1.0e-3f < cal->_delay_start_sec) {
                return false;
            }

            if (cal->_sample_buffer == NULL) {
                cal->_sample_buffer = (CompassSample*)rt_calloc(COMPASS_CAL_NUM_SAMPLES, sizeof(CompassSample));
            }
            if (cal->_sample_buffer != NULL) {
                initialize_fit(cal);
                cal->_status = COMPASS_CAL_RUNNING_STEP_ONE;
                return true;
            } else {
                COMPASS_CAL_GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mag(%u) bad allocate _sample_buffer", cal->_compass_idx);
            }
            return false;

        case COMPASS_CAL_RUNNING_STEP_TWO:
            if (cal->_status != COMPASS_CAL_RUNNING_STEP_ONE) {
                return false;
            }
            thin_samples(cal);
            initialize_fit(cal);
            cal->_status = COMPASS_CAL_RUNNING_STEP_TWO;
            return true;

        case COMPASS_CAL_SUCCESS:
            if (cal->_status != COMPASS_CAL_RUNNING_STEP_TWO) {
                return false;
            }

            if (cal->_sample_buffer != NULL) {
                rt_free(cal->_sample_buffer);
                cal->_sample_buffer = NULL;
            }

            cal->_status = COMPASS_CAL_SUCCESS;
            return true;

        case COMPASS_CAL_FAILED:
            if (cal->_status == COMPASS_CAL_BAD_ORIENTATION ||
                cal->_status == COMPASS_CAL_BAD_RADIUS) {
                // don't overwrite bad orientation status
                return false;
            }

        case COMPASS_CAL_BAD_ORIENTATION:
        case COMPASS_CAL_BAD_RADIUS:
            if (cal->_status == COMPASS_CAL_NOT_STARTED) {
                return false;
            }

            if (cal->_retry && set_status(cal, COMPASS_CAL_WAITING_TO_START)) {
                cal->_attempt++;
                return true;
            }

            if (cal->_sample_buffer != NULL) {
                rt_free(cal->_sample_buffer);
                cal->_sample_buffer = NULL;
            }

            cal->_status = status;
            return true;

        default:
            return false;
    };
}

static void reset_state(CompassCalibrator *cal)
{
    cal->_samples_collected = 0;
    cal->_samples_thinned = 0;
    cal->_params.radius = 200;
    vec3_zero(&cal->_params.offset);
    vec3_set(&cal->_params.diag, 1.0f, 1.0f, 1.0f);
    vec3_zero(&cal->_params.offdiag);
    cal->_params.scale_factor = 0;

    rt_memset(cal->_completion_mask, 0, sizeof(cal->_completion_mask));
    initialize_fit(cal);
}

// initialize fitness before starting a fit
static void initialize_fit(CompassCalibrator *cal)
{
    if (cal->_samples_collected != 0) {
        cal->_fitness = calc_mean_squared_residuals(cal, &cal->_params);
    } else {
        cal->_fitness = 1.0e30f;
    }
    cal->_initial_fitness = cal->_fitness;
    cal->_sphere_lambda = 1.0f;
    cal->_ellipsoid_lambda = 1.0f;
    cal->_fit_step = 0;
}

// calc the fitness given a set of parameters (offsets, diagonals, off diagonals)
static float calc_mean_squared_residuals(CompassCalibrator *cal, const CompassCalParam* params)
{
    if (cal->_sample_buffer == NULL || cal->_samples_collected == 0) {
        return 1.0e30f;
    }
    float sum = 0.0f;
    for (uint16_t i=0; i < cal->_samples_collected; i++) {
        Vector3f_t sample = CompassSample_get(&cal->_sample_buffer[i]);
        float resid = calc_residual(&sample, params);
        sum += sq(resid);
    }
    sum /= cal->_samples_collected;
    return sum;
}

static float calc_residual(const Vector3f_t* sample, const CompassCalParam* params)
{
    matrix3f_t softiron = {
        {params->diag.x    , params->offdiag.x , params->offdiag.y},
        {params->offdiag.x , params->diag.y    , params->offdiag.z},
        {params->offdiag.y , params->offdiag.z , params->diag.z}
    };

    // 修正偏移量后的数据
    Vector3f_t sample_cor = {sample->x + params->offset.x,
                             sample->y + params->offset.y,
                             sample->z + params->offset.z,};

    // 正交修正后的数据
    Vector3f_t sample_cor2;
    matrix3f_mul_vec(&sample_cor2, &softiron, &sample_cor);

    return params->radius - vec3_length(&sample_cor2);
}

static void thin_samples(CompassCalibrator *cal)
{
    if (cal->_sample_buffer == NULL) {
        return;
    }

    cal->_samples_thinned = 0;
    // shuffle the samples http://en.wikipedia.org/wiki/Fisher%E2%80%93Yates_shuffle
    // this is so that adjacent samples don't get sequentially eliminated
    for (uint16_t i=cal->_samples_collected-1; i>=1; i--) {
        uint16_t j = math_get_random16() % (i+1);
        CompassSample temp = cal->_sample_buffer[i];
        cal->_sample_buffer[i] = cal->_sample_buffer[j];
        cal->_sample_buffer[j] = temp;
    }

    // remove any samples that are close together
    for (uint16_t i=0; i < cal->_samples_collected; i++) {
        if (!accept_sample(cal, &cal->_sample_buffer[i], i)) {
            cal->_sample_buffer[i] = cal->_sample_buffer[cal->_samples_collected-1];
            cal->_samples_collected--;
            cal->_samples_thinned++;
        }
    }

    update_completion_mask(cal);
}

static bool accept_sample(CompassCalibrator *cal, const CompassSample* sample, uint16_t skip_index)
{
    Vector3f_t _sample = CompassSample_get(sample);
    return accept_sample2(cal, &_sample, skip_index);
}

/*
 * The sample acceptance distance is determined as follows:
 * For any regular polyhedron with triangular faces, the angle theta subtended
 * by two closest points is defined as
 *
 *      theta = arccos(cos(A)/(1-cos(A)))
 *
 * Where:
 *      A = (4pi/F + pi)/3
 * and
 *      F = 2V - 4 is the number of faces for the polyhedron in consideration,
 *      which depends on the number of vertices V
 *
 * The above equation was proved after solving for spherical triangular excess
 * and related equations.
 */
static bool accept_sample2(CompassCalibrator *cal, const Vector3f_t* sample, uint16_t skip_index)
{
    static const uint16_t faces = (2 * COMPASS_CAL_NUM_SAMPLES - 4);
    static const float a = (4.0f * M_PI / (3.0f * faces)) + M_PI / 3.0f;
    const float theta = 0.5f * acosf(cosf(a) / (1.0f - cosf(a)));

    if (cal->_sample_buffer == NULL) {
        return false;
    }

    float min_distance = cal->_params.radius * 2*sinf(theta/2);

    for (uint16_t i = 0; i<cal->_samples_collected; i++) {
        Vector3f_t mag_sample = CompassSample_get(&cal->_sample_buffer[i]);
        Vector3f_t sample_cor = {sample->x - mag_sample.x,
                                 sample->y - mag_sample.y,
                                 sample->z - mag_sample.z};
            
        if (i != skip_index) {
            float distance = vec3_length(&sample_cor);
            if (distance < min_distance) {
                return false;
            }
        }
    }
    return true;
}

// reset and update the completion mask using all samples in the sample buffer
static void update_completion_mask(CompassCalibrator *cal)
{
    rt_memset(cal->_completion_mask, 0, sizeof(cal->_completion_mask));
    for (int i = 0; i < cal->_samples_collected; i++) {
        Vector3f_t sample = CompassSample_get(&cal->_sample_buffer[i]);
        update_completion_mask2(cal, &sample);
    }
}

// update completion mask based on latest sample
// used to ensure we have collected samples in all directions
static void update_completion_mask2(CompassCalibrator *cal, const Vector3f_t* v)
{
    matrix3f_t softiron  = {
        {cal->_params.diag.x,    cal->_params.offdiag.x, cal->_params.offdiag.y},
        {cal->_params.offdiag.x, cal->_params.diag.y,    cal->_params.offdiag.z},
        {cal->_params.offdiag.y, cal->_params.offdiag.z, cal->_params.diag.z}
    };
    Vector3f_t v_cor;
    vec3_add(&v_cor, v, &cal->_params.offset);

    Vector3f_t corrected;
    matrix3f_mul_vec(&corrected, &softiron, &v_cor);

    int section = geogrid_section(&corrected, true);
    if (section < 0) {
        return;
    }
    cal->_completion_mask[section / 8] |= 1 << (section % 8);
}

// running method for use in thread
static bool _running(CompassCalibrator *cal)
{
    return cal->_status == COMPASS_CAL_RUNNING_STEP_ONE || cal->_status == COMPASS_CAL_RUNNING_STEP_TWO;
}

static void update_cal_settings(CompassCalibrator *cal)
{
    cal->_tolerance = cal->cal_settings.tolerance;
    cal->_check_orientation = cal->cal_settings.check_orientation;
    cal->_orientation = cal->cal_settings.orientation;
    cal->_orig_orientation = cal->cal_settings.orig_orientation;
    cal->_is_external = cal->cal_settings.is_external;
    cal->_fix_orientation = cal->cal_settings.fix_orientation;
    cal->_offset_max = cal->cal_settings.offset_max;
    cal->_attempt = cal->cal_settings.attempt;
    cal->_retry = cal->cal_settings.retry;
    cal->_delay_start_sec = cal->cal_settings.delay_start_sec;
    cal->_start_time_ms = cal->cal_settings.start_time_ms;
    cal->_compass_idx = cal->cal_settings.compass_idx;
    cal->_always_45_deg = cal->cal_settings.always_45_deg;
}

static void update_cal_status(CompassCalibrator *cal)
{
    cal->cal_state.status = cal->_status;
    cal->cal_state.attempt = cal->_attempt;
    rt_memcpy(cal->cal_state.completion_mask, cal->_completion_mask, sizeof(completion_mask_t));
    cal->cal_state.completion_pct = 0.0f;
    // first sampling step is 1/3rd of the progress bar
    // never return more than 99% unless _status is Status::SUCCESS
    switch (cal->_status) {
        case COMPASS_CAL_NOT_STARTED:
        case COMPASS_CAL_WAITING_TO_START:
            cal->cal_state.completion_pct = 0.0f;
            break;
        case COMPASS_CAL_RUNNING_STEP_ONE:
            cal->cal_state.completion_pct = 33.3f * cal->_samples_collected/COMPASS_CAL_NUM_SAMPLES;
            break;
        case COMPASS_CAL_RUNNING_STEP_TWO:
            cal->cal_state.completion_pct = 33.3f + 65.7f*((float)(cal->_samples_collected-cal->_samples_thinned)/(COMPASS_CAL_NUM_SAMPLES-cal->_samples_thinned));
            break;
        case COMPASS_CAL_SUCCESS:
            cal->cal_state.completion_pct = 100.0f;
            break;
        case COMPASS_CAL_FAILED:
        case COMPASS_CAL_BAD_ORIENTATION:
        case COMPASS_CAL_BAD_RADIUS:
            cal->cal_state.completion_pct = 0.0f;
            break;
    };
}

static void update_cal_report(CompassCalibrator *cal)
{
    cal->cal_report.status = cal->_status;
    cal->cal_report.fitness = sqrtf(cal->_fitness);
    cal->cal_report.ofs = cal->_params.offset;
    cal->cal_report.diag = cal->_params.diag;
    cal->cal_report.offdiag = cal->_params.offdiag;
    cal->cal_report.scale_factor = cal->_params.scale_factor;
    cal->cal_report.orientation_confidence = cal->_orientation_confidence;
    cal->cal_report.original_orientation = cal->_orig_orientation;
    cal->cal_report.orientation = cal->_orientation_solution;
    cal->cal_report.check_orientation = cal->_check_orientation;
}

// fitting method for use in thread
static bool _fitting(CompassCalibrator *cal)
{
    return _running(cal) && (cal->_samples_collected == COMPASS_CAL_NUM_SAMPLES);
}

// calculate initial offsets by simply taking the average values of the samples
static void calc_initial_offset(CompassCalibrator *cal)
{
    // Set initial offset to the average value of the samples
    vec3_zero(&cal->_params.offset);

    Vector3f_t sample;
    for (uint16_t k = 0; k < cal->_samples_collected; k++) {
        sample = CompassSample_get(&cal->_sample_buffer[k]);

        vec3_sub(&cal->_params.offset, &cal->_params.offset, &sample);
    }

    vec3_mult(&cal->_params.offset, &cal->_params.offset, 1.0f/cal->_samples_collected);
}

// run sphere fit to calculate diagonals and offdiagonals
static void run_sphere_fit(CompassCalibrator *cal)
{
    if (cal->_sample_buffer == NULL) {
        return;
    }

    const float lma_damping = 10.0f;

    // take backup of fitness and parameters so we can determine later if this fit has improved the calibration
    float fitness = cal->_fitness;
    float fit1, fit2;
    CompassCalParam fit1_params, fit2_params;
    fit1_params = fit2_params = cal->_params;

    float JTJ[COMPASS_CAL_NUM_SPHERE_PARAMS*COMPASS_CAL_NUM_SPHERE_PARAMS] = {0};
    float JTJ2[COMPASS_CAL_NUM_SPHERE_PARAMS*COMPASS_CAL_NUM_SPHERE_PARAMS] = {0};
    float JTFI[COMPASS_CAL_NUM_SPHERE_PARAMS] = {0};

    // Gauss Newton Part common for all kind of extensions including LM
    for (uint16_t k = 0; k<cal->_samples_collected; k++) {
        Vector3f_t sample = CompassSample_get(&cal->_sample_buffer[k]);

        float sphere_jacob[COMPASS_CAL_NUM_SPHERE_PARAMS];

        calc_sphere_jacob(cal, &sample, &fit1_params, sphere_jacob);

        for (uint8_t i = 0;i < COMPASS_CAL_NUM_SPHERE_PARAMS; i++) {
            // compute JTJ
            for (uint8_t j = 0; j < COMPASS_CAL_NUM_SPHERE_PARAMS; j++) {
                JTJ[i*COMPASS_CAL_NUM_SPHERE_PARAMS+j] += sphere_jacob[i] * sphere_jacob[j];
                JTJ2[i*COMPASS_CAL_NUM_SPHERE_PARAMS+j] += sphere_jacob[i] * sphere_jacob[j];   //a backup JTJ for LM
            }
            // compute JTFI
            JTFI[i] += sphere_jacob[i] * calc_residual(&sample, &fit1_params);
        }
    }

    //------------------------Levenberg-Marquardt-part-starts-here---------------------------------//
    // refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
    for (uint8_t i = 0; i < COMPASS_CAL_NUM_SPHERE_PARAMS; i++) {
        JTJ[i*COMPASS_CAL_NUM_SPHERE_PARAMS+i] += cal->_sphere_lambda;
        JTJ2[i*COMPASS_CAL_NUM_SPHERE_PARAMS+i] += cal->_sphere_lambda/lma_damping;
    }

    if (!mat_inverse(JTJ, JTJ, 4)) {
        return;
    }

    if (!mat_inverse(JTJ2, JTJ2, 4)) {
        return;
    }

    // extract radius, offset, diagonals and offdiagonal parameters
    for (uint8_t row=0; row < COMPASS_CAL_NUM_SPHERE_PARAMS; row++) {
        for (uint8_t col=0; col < COMPASS_CAL_NUM_SPHERE_PARAMS; col++) {
            ((float *)&fit1_params.radius)[row] -= JTFI[col] * JTJ[row*COMPASS_CAL_NUM_SPHERE_PARAMS+col];
            ((float *)&fit2_params.radius)[row] -= JTFI[col] * JTJ2[row*COMPASS_CAL_NUM_SPHERE_PARAMS+col];
        }
    }

    // calculate fitness of two possible sets of parameters
    fit1 = calc_mean_squared_residuals(cal, &fit1_params);
    fit2 = calc_mean_squared_residuals(cal, &fit2_params);

    // decide which of the two sets of parameters is best and store in fit1_params
    if (fit1 > cal->_fitness && fit2 > cal->_fitness) {
        // if neither set of parameters provided better results, increase lambda
        cal->_sphere_lambda *= lma_damping;
    } else if (fit2 < cal->_fitness && fit2 < fit1) {
        // if fit2 was better we will use it. decrease lambda
        cal->_sphere_lambda /= lma_damping;
        fit1_params = fit2_params;
        fitness = fit2;
    } else if (fit1 < cal->_fitness) {
        fitness = fit1;
    }
    //--------------------Levenberg-Marquardt-part-ends-here--------------------------------//

    // store new parameters and update fitness
    if (!isnan(fitness) && fitness < cal->_fitness) {
        cal->_fitness = fitness;
        cal->_params = fit1_params;
        update_completion_mask(cal);
    }
}

static void calc_sphere_jacob(CompassCalibrator *cal, const Vector3f_t* sample, const CompassCalParam* params, float* ret)
{
    const Vector3f_t *offset = &params->offset;
    const Vector3f_t *diag = &params->diag;
    const Vector3f_t *offdiag = &params->offdiag;
    matrix3f_t softiron = {
        {diag->x    , offdiag->x , offdiag->y},
        {offdiag->x , diag->y    , offdiag->z},
        {offdiag->y , offdiag->z , diag->z}
    };

    float A =  (diag->x    * (sample->x + offset->x)) + (offdiag->x * (sample->y + offset->y)) + (offdiag->y * (sample->z + offset->z));
    float B =  (offdiag->x * (sample->x + offset->x)) + (diag->y    * (sample->y + offset->y)) + (offdiag->z * (sample->z + offset->z));
    float C =  (offdiag->y * (sample->x + offset->x)) + (offdiag->z * (sample->y + offset->y)) + (diag->z    * (sample->z + offset->z));

    Vector3f_t sample_cor;
    vec3_add(&sample_cor, sample, offset);

    Vector3f_t sample_cor2;
    matrix3f_mul_vec(&sample_cor2, &softiron, &sample_cor);

    float length = vec3_length(&sample_cor2);

    // 0: partial derivative (radius wrt fitness fn) fn operated on sample
    ret[0] = 1.0f;
    // 1-3: partial derivative (offsets wrt fitness fn) fn operated on sample
    ret[1] = -1.0f * (((diag->x    * A) + (offdiag->x * B) + (offdiag->y * C))/length);
    ret[2] = -1.0f * (((offdiag->x * A) + (diag->y    * B) + (offdiag->z * C))/length);
    ret[3] = -1.0f * (((offdiag->y * A) + (offdiag->z * B) + (diag->z    * C))/length);
}

static bool fit_acceptable(CompassCalibrator *cal)
{
    if (!isnan(cal->_fitness) &&
        cal->_params.radius > FIELD_RADIUS_MIN && cal->_params.radius < FIELD_RADIUS_MAX &&
        fabsf(cal->_params.offset.x) < cal->_offset_max &&
        fabsf(cal->_params.offset.y) < cal->_offset_max &&
        fabsf(cal->_params.offset.z) < cal->_offset_max &&
        cal->_params.diag.x > 0.2f && cal->_params.diag.x < 5.0f &&
        cal->_params.diag.y > 0.2f && cal->_params.diag.y < 5.0f &&
        cal->_params.diag.z > 0.2f && cal->_params.diag.z < 5.0f &&
        fabsf(cal->_params.offdiag.x) < 1.0f &&      //absolute of sine/cosine output cannot be greater than 1
        fabsf(cal->_params.offdiag.y) < 1.0f &&
        fabsf(cal->_params.offdiag.z) < 1.0f ) {
            return cal->_fitness <= sq(cal->_tolerance);
        }
    return false;
}

/*
  fix radius of the fit to compensate for sensor scale factor errors
  return false if radius is outside acceptable range
 */
static bool fix_radius(CompassCalibrator *cal)
{
    uitc_sensor_gps gps_pos_t = {0};
    int res = itc_copy_from_hub(ITC_ID(sensor_gps), &gps_pos_t);

    if (res != 0 || gps_pos_t.fix_type < 2) {
        // we don't have a position, leave scale factor as 0. This
        // will disable use of WMM in the EKF. Users can manually set
        // scale factor after calibration if it is known
        cal->_params.scale_factor = 0;
        return true;
    }

    float intensity;
    float declination;
    float inclination;
    geo_get_mag_field_ef(gps_pos_t.lat * 1e-7f, gps_pos_t.lon * 1e-7f, &intensity, &declination, &inclination);

    float expected_radius = intensity * 1000; // mGauss
    float correction = expected_radius / cal->_params.radius;

    if (correction > COMPASS_MAX_SCALE_FACTOR || correction < COMPASS_MIN_SCALE_FACTOR) {
        // don't allow more than 30% scale factor correction
        COMPASS_CAL_GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mag(%u) bad radius %.0f expected %.0f",
                        cal->_compass_idx,
                        cal->_params.radius,
                        expected_radius);
        set_status(cal, COMPASS_CAL_BAD_RADIUS);
        return false;
    }

    cal->_params.scale_factor = correction;

    return true;
}

/*
  calculate compass orientation using the attitude estimate associated
  with each sample, and fix orientation on external compasses if
  the feature is enabled
 */
static bool calculate_orientation(CompassCalibrator *cal)
{
    if (!cal->_check_orientation) {
        // we are not checking orientation
        return true;
    }

    // this function is very slow
    //EXPECT_DELAY_MS(1000);
    //rt_thread_mdelay(1000);

    // Skip 4 rotations, see: auto_rotation_index()
    float variance[ROTATION_MAX-4] = {0};

    cal->_orientation_solution = cal->_orientation;
    
    for (uint8_t n=0; n < ARRAY_SIZE(variance); n++) {
        enum RotationEnum r = compasscal_auto_rotation_index(n);

        // calculate the average implied earth field across all samples
        Vector3f_t total_ef  = {0};
        for (uint32_t i=0; i<cal->_samples_collected; i++) {
            Vector3f_t efield = calculate_earth_field(cal, &cal->_sample_buffer[i], r);
            //total_ef += efield;
            vec3_add(&total_ef, &total_ef, &efield);
        }
        //Vector3f_t avg_efield = total_ef / cal->_samples_collected;
        Vector3f_t avg_efield;
        vec3_mult(&avg_efield, &total_ef, 1.0f/cal->_samples_collected);

        // now calculate the square error for this rotation against the average earth field
        for (uint32_t i=0; i<cal->_samples_collected; i++) {
            Vector3f_t efield = calculate_earth_field(cal, &cal->_sample_buffer[i], r);
            //float err = (efield - avg_efield).length_squared();
            Vector3f_t efield_cor = {efield.x - avg_efield.x, efield.y - avg_efield.y, efield.z - avg_efield.z};
            float err = vec3_length_squared(&efield_cor);
            // divide by number of samples collected to get the variance
            variance[n] += err / cal->_samples_collected;
        }
    }

    // find the rotation with the lowest variance
    enum RotationEnum besti = ROTATION_NONE;
    float bestv = variance[0];
    enum RotationEnum besti_90 = ROTATION_NONE;
    float bestv_90 = variance[0];
    for (uint8_t n=0; n < ARRAY_SIZE(variance); n++) {
        enum RotationEnum r = compasscal_auto_rotation_index(n);
        if (variance[n] < bestv) {
            bestv = variance[n];
            besti = r;
        }
        if (right_angle_rotation(r) && variance[n] < bestv_90) {
            bestv_90 = variance[n];
            besti_90 = r;
        }
    }


    float second_best = besti==ROTATION_NONE?variance[1]:variance[0];
    enum RotationEnum besti2 = besti==ROTATION_NONE?ROTATION_YAW_45:ROTATION_NONE;
    float second_best_90 = besti_90==ROTATION_NONE?variance[2]:variance[0];
    enum RotationEnum besti2_90 = besti_90==ROTATION_NONE?ROTATION_YAW_90:ROTATION_NONE;
    for (uint8_t n=0; n < ARRAY_SIZE(variance); n++) {
        enum RotationEnum r = compasscal_auto_rotation_index(n);
        if (besti != r) {
            if (variance[n] < second_best) {
                second_best = variance[n];
                besti2 = r;
            }
        }
        if (right_angle_rotation(r) && (besti_90 != r) && (variance[n] < second_best_90)) {
            second_best_90 = variance[n];
            besti2_90 = r;
        }
    }

    cal->_orientation_confidence = second_best/bestv;

    bool pass;
    if (besti == cal->_orientation) {
        // if the orientation matched then allow for a low threshold
        pass = true;
    } else {
        if (cal->_orientation_confidence > 4.0) {
            // very confident, always pass
            pass = true;

        } else if (cal->_always_45_deg && (cal->_orientation_confidence > 2.0)) {
            // use same tolerance for all rotations
            pass = true;

        } else {
            // just consider 90's
            cal->_orientation_confidence = second_best_90 / bestv_90;
            pass = cal->_orientation_confidence > 2.0;
            besti = besti_90;
            besti2 = besti2_90;
        }
    }
    if (!pass) {
        COMPASS_CAL_GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Mag(%u) bad orientation: %u/%u %.1f", cal->_compass_idx,
                        besti, besti2, (double)cal->_orientation_confidence);
        (void)besti2;
    } else if (besti == cal->_orientation) {
        // no orientation change
        COMPASS_CAL_GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mag(%u) good orientation: %u %.1f", cal->_compass_idx, besti, (double)cal->_orientation_confidence);
    } else if (!cal->_is_external || !cal->_fix_orientation) {
        COMPASS_CAL_GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Mag(%u) internal bad orientation: %u %.1f", cal->_compass_idx, besti, (double)cal->_orientation_confidence);
    } else {
        COMPASS_CAL_GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mag(%u) new orientation: %u was %u %.1f", cal->_compass_idx, besti, cal->_orientation, (double)cal->_orientation_confidence);
    }

    if (!pass) {
        set_status(cal, COMPASS_CAL_BAD_ORIENTATION);
        return false;
    }

    if (cal->_orientation == besti) {
        // no orientation change
        return true;
    }

    if (!cal->_is_external || !cal->_fix_orientation) {
        // we won't change the orientation, but we set _orientation
        // for reporting purposes
        cal->_orientation = besti;
        cal->_orientation_solution = besti;
        set_status(cal, COMPASS_CAL_BAD_ORIENTATION);
        return false;
    }

    // correct the offsets for the new orientation
    Vector3f_t rot_offsets = cal->_params.offset;
    //rot_offsets.rotate_inverse(_orientation);
    //rot_offsets.rotate(besti);
    vec3_rotate_inverse(&rot_offsets, cal->_orientation);
    vec3_rotate(&rot_offsets, besti);

    cal->_params.offset = rot_offsets;

    // rotate the samples for the new orientation
    for (uint32_t i=0; i<cal->_samples_collected; i++) {
        Vector3f_t s = CompassSample_get(&cal->_sample_buffer[i]);
        vec3_rotate_inverse(&s, cal->_orientation);
        vec3_rotate(&s, besti);

        CompassSample_set(&cal->_sample_buffer[i], &s);
    }

    cal->_orientation = besti;
    cal->_orientation_solution = besti;

    // re-run the fit to get the diagonals and off-diagonals for the
    // new orientation
    initialize_fit(cal);
    run_sphere_fit(cal);
    run_ellipsoid_fit(cal);

    return fit_acceptable(cal);
}

/*
  calculate the implied earth field for a compass sample and compass
  rotation. This is used to check for consistency between
  samples. 

  If the orientation is correct then when rotated the same (or
  similar) earth field should be given for all samples. 

  Note that this earth field uses an arbitrary north reference, so it
  may not match the true earth field.
 */
static Vector3f_t calculate_earth_field(CompassCalibrator *cal, CompassSample *sample, enum RotationEnum r)
{
    Vector3f_t v = CompassSample_get(sample);

    // convert the sample back to sensor frame
    // v.rotate_inverse(_orientation);
    vec3_rotate_inverse(&v, cal->_orientation);

    // rotate to body frame for this rotation
    // v.rotate(r);
    vec3_rotate(&v, r);

    // apply offsets, rotating them for the orientation we are testing
    Vector3f_t rot_offsets = cal->_params.offset;
    //rot_offsets.rotate_inverse(_orientation);
    vec3_rotate_inverse(&rot_offsets, cal->_orientation);

    //rot_offsets.rotate(r);
    vec3_rotate(&rot_offsets, r);

    //v += rot_offsets;
    vec3_add(&v, &v, &rot_offsets);

    // rotate the sample from body frame back to earth frame
    matrix3f_t rot = AttitudeSample_get_rotmat(&sample->att);

    Vector3f_t efield;
    matrix3f_mul_vec(&efield, &rot, &v);

    // earth field is the mag sample in earth frame
    return efield;
}

static bool right_angle_rotation(enum RotationEnum r)
{
    switch (r) {
        case ROTATION_NONE:
        case ROTATION_YAW_90:
        case ROTATION_YAW_180:
        case ROTATION_YAW_270:
        case ROTATION_ROLL_180:
        case ROTATION_ROLL_180_YAW_90:
        case ROTATION_PITCH_180:
        case ROTATION_ROLL_180_YAW_270:
        case ROTATION_ROLL_90:
        case ROTATION_ROLL_90_YAW_90:
        case ROTATION_ROLL_270:
        case ROTATION_ROLL_270_YAW_90:
        case ROTATION_PITCH_90:
        case ROTATION_PITCH_270:
        case ROTATION_PITCH_180_YAW_90:
        case ROTATION_PITCH_180_YAW_270:
        case ROTATION_ROLL_90_PITCH_90:
        case ROTATION_ROLL_180_PITCH_90:
        case ROTATION_ROLL_270_PITCH_90:
        case ROTATION_ROLL_90_PITCH_180:
        case ROTATION_ROLL_270_PITCH_180:
        case ROTATION_ROLL_90_PITCH_270:
        case ROTATION_ROLL_180_PITCH_270:
        case ROTATION_ROLL_270_PITCH_270:
        case ROTATION_ROLL_90_PITCH_180_YAW_90:
        case ROTATION_ROLL_90_YAW_270:
            return true;

        default:
            return false;
    }
}

static void run_ellipsoid_fit(CompassCalibrator *cal)
{
    if (cal->_sample_buffer == NULL) {
        return;
    }

    const float lma_damping = 10.0f;

    // take backup of fitness and parameters so we can determine later if this fit has improved the calibration
    float fitness = cal->_fitness;
    float fit1, fit2;
    CompassCalParam fit1_params, fit2_params;
    fit1_params = fit2_params = cal->_params;

    float JTJ[COMPASS_CAL_NUM_ELLIPSOID_PARAMS*COMPASS_CAL_NUM_ELLIPSOID_PARAMS] = { };
    float JTJ2[COMPASS_CAL_NUM_ELLIPSOID_PARAMS*COMPASS_CAL_NUM_ELLIPSOID_PARAMS] = { };
    float JTFI[COMPASS_CAL_NUM_ELLIPSOID_PARAMS] = { };

    // Gauss Newton Part common for all kind of extensions including LM
    for (uint16_t k = 0; k<cal->_samples_collected; k++) {
        Vector3f_t sample = CompassSample_get(&cal->_sample_buffer[k]);

        float ellipsoid_jacob[COMPASS_CAL_NUM_ELLIPSOID_PARAMS];

        calc_ellipsoid_jacob(cal, &sample, &fit1_params, ellipsoid_jacob);

        for (uint8_t i = 0;i < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; i++) {
            // compute JTJ
            for (uint8_t j = 0; j < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; j++) {
                JTJ [i*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+j] += ellipsoid_jacob[i] * ellipsoid_jacob[j];
                JTJ2[i*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+j] += ellipsoid_jacob[i] * ellipsoid_jacob[j];
            }
            // compute JTFI
            JTFI[i] += ellipsoid_jacob[i] * calc_residual(&sample, &fit1_params);
        }
    }

    //------------------------Levenberg-Marquardt-part-starts-here---------------------------------//
    //refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
    for (uint8_t i = 0; i < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; i++) {
        JTJ[i*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+i] += cal->_ellipsoid_lambda;
        JTJ2[i*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+i] += cal->_ellipsoid_lambda/lma_damping;
    }

    if (!mat_inverse(JTJ, JTJ, 9)) {
        return;
    }

    if (!mat_inverse(JTJ2, JTJ2, 9)) {
        return;
    }

    // extract radius, offset, diagonals and offdiagonal parameters
    for (uint8_t row=0; row < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; row++) {
        for (uint8_t col=0; col < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; col++) {
            ((float *)&fit1_params.offset.x)[row] -= JTFI[col] * JTJ[row*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+col];
            ((float *)&fit2_params.offset.x)[row] -= JTFI[col] * JTJ2[row*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+col];
        }
    }

    // calculate fitness of two possible sets of parameters
    fit1 = calc_mean_squared_residuals(cal, &fit1_params);
    fit2 = calc_mean_squared_residuals(cal, &fit2_params);

    // decide which of the two sets of parameters is best and store in fit1_params
    if (fit1 > cal->_fitness && fit2 > cal->_fitness) {
        // if neither set of parameters provided better results, increase lambda
        cal->_ellipsoid_lambda *= lma_damping;
    } else if (fit2 < cal->_fitness && fit2 < fit1) {
        // if fit2 was better we will use it. decrease lambda
        cal->_ellipsoid_lambda /= lma_damping;
        fit1_params = fit2_params;
        fitness = fit2;
    } else if (fit1 < cal->_fitness) {
        fitness = fit1;
    }
    //--------------------Levenberg-part-ends-here--------------------------------//

    // store new parameters and update fitness
    if (fitness < cal->_fitness) {
        cal->_fitness = fitness;
        cal->_params = fit1_params;
        update_completion_mask(cal);
    }
}

static void calc_ellipsoid_jacob(CompassCalibrator *cal, const Vector3f_t* sample, const CompassCalParam* params, float* ret)
{
    const Vector3f_t *offset = &params->offset;
    const Vector3f_t *diag = &params->diag;
    const Vector3f_t *offdiag = &params->offdiag;
    matrix3f_t softiron = {
        diag->x    , offdiag->x , offdiag->y,
        offdiag->x , diag->y    , offdiag->z,
        offdiag->y , offdiag->z , diag->z
    };

    float A =  (diag->x    * (sample->x + offset->x)) + (offdiag->x * (sample->y + offset->y)) + (offdiag->y * (sample->z + offset->z));
    float B =  (offdiag->x * (sample->x + offset->x)) + (diag->y    * (sample->y + offset->y)) + (offdiag->z * (sample->z + offset->z));
    float C =  (offdiag->y * (sample->x + offset->x)) + (offdiag->z * (sample->y + offset->y)) + (diag->z    * (sample->z + offset->z));
    //float length = (softiron*(sample+offset)).length();

    Vector3f_t sample_cor;
    vec3_add(&sample_cor, sample, offset);

    Vector3f_t sample_cor2;
    matrix3f_mul_vec(&sample_cor2, &softiron, &sample_cor);

    float length = vec3_length(&sample_cor2);

    // 0-2: partial derivative (offset wrt fitness fn) fn operated on sample
    ret[0] = -1.0f * (((diag->x    * A) + (offdiag->x * B) + (offdiag->y * C))/length);
    ret[1] = -1.0f * (((offdiag->x * A) + (diag->y    * B) + (offdiag->z * C))/length);
    ret[2] = -1.0f * (((offdiag->y * A) + (offdiag->z * B) + (diag->z    * C))/length);
    // 3-5: partial derivative (diag offset wrt fitness fn) fn operated on sample
    ret[3] = -1.0f * ((sample->x + offset->x) * A)/length;
    ret[4] = -1.0f * ((sample->y + offset->y) * B)/length;
    ret[5] = -1.0f * ((sample->z + offset->z) * C)/length;
    // 6-8: partial derivative (off-diag offset wrt fitness fn) fn operated on sample
    ret[6] = -1.0f * (((sample->y + offset->y) * A) + ((sample->x + offset->x) * B))/length;
    ret[7] = -1.0f * (((sample->z + offset->z) * A) + ((sample->x + offset->x) * C))/length;
    ret[8] = -1.0f * (((sample->z + offset->z) * B) + ((sample->y + offset->y) * C))/length;
}

//////////////////////////////////////////////////////////
//////////// CompassSample public interface //////////////
//////////////////////////////////////////////////////////

static Vector3f_t CompassSample_get(const CompassSample *sample_buffer)
{
    Vector3f_t sample = {COMPASS_CAL_SAMPLE_SCALE_TO_FLOAT(sample_buffer->x),
                         COMPASS_CAL_SAMPLE_SCALE_TO_FLOAT(sample_buffer->y),
                         COMPASS_CAL_SAMPLE_SCALE_TO_FLOAT(sample_buffer->z)};
    return sample;
}

static void CompassSample_set(CompassSample *sample_buffer, const Vector3f_t *in)
{
    sample_buffer->x = COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(in->x);
    sample_buffer->y = COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(in->y);
    sample_buffer->z = COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(in->z);
}

static void AttitudeSample_set_from_ahrs(CompassCalAttSample *att_sam)
{
    uitc_vehicle_attitude vehicle_attitude;
    if (itc_copy_from_hub(ITC_ID(vehicle_attitude),&vehicle_attitude) != 0) {
        att_sam->roll = 0;
        att_sam->pitch = 0;
        att_sam->yaw = 0;

        return;
    }

    float roll_rad = vehicle_attitude.vehicle_euler.roll;
    float pitch_rad = vehicle_attitude.vehicle_euler.pitch;
    float yaw_rad = vehicle_attitude.vehicle_euler.yaw;

    att_sam->roll = math_constrain_int16(127 * (roll_rad / M_PI), -127, 127);
    att_sam->pitch = math_constrain_int16(127 * (pitch_rad / M_PI_2), -127, 127);
    att_sam->yaw = math_constrain_int16(127 * (yaw_rad / M_PI), -127, 127);
}

static matrix3f_t AttitudeSample_get_rotmat(CompassCalAttSample *att_sam)
{
    float roll_rad, pitch_rad, yaw_rad;
    roll_rad = att_sam->roll * (M_PI / 127);
    pitch_rad = att_sam->pitch * (M_PI_2 / 127);
    yaw_rad = att_sam->yaw * (M_PI / 127);

    matrix3f_t dcm;
    matrix3f_from_euler(&dcm, roll_rad, pitch_rad, yaw_rad);

    return dcm;
}

/*------------------------------------test------------------------------------*/


